/*
 * Academic License - for use in teaching, academic research, and meeting
 * course requirements at degree granting institutions only.  Not for
 * government, commercial, or other organizational use.
 *
 * File: Current_Loop.c
 *
 * Code generated for Simulink model 'IF_Control'.
 *
 * Model version                  : 1.49
 * Simulink Coder version         : 9.8 (R2022b) 13-May-2022
 * C/C++ source code generated on : Tue Sep 10 22:33:31 2024
 *
 * Target selection: ert.tlc
 * Embedded hardware selection: ARM Compatible->ARM Cortex-M
 * Code generation objectives: Unspecified
 * Validation result: Not run
 */

#include "rtwtypes.h"
#include "Current_Loop.h"
#include "IF_Control_private.h"
#include <math.h>
#include "rt_nonfinite.h"
#include "IF_Control.h"
#include <float.h>

real32_T rt_modf_snf(real32_T u0, real32_T u1)
{
  real32_T y;
  y = u0;
  if (u1 == 0.0F) {
    if (u0 == 0.0F) {
      y = u1;
    }
  } else if (rtIsNaNF(u0) || rtIsNaNF(u1) || rtIsInfF(u0)) {
    y = (rtNaNF);
  } else if (u0 == 0.0F) {
    y = 0.0F / u1;
  } else if (rtIsInfF(u1)) {
    if ((u1 < 0.0F) != (u0 < 0.0F)) {
      y = u1;
    }
  } else {
    boolean_T yEq;
    y = fmodf(u0, u1);
    yEq = (y == 0.0F);
    if ((!yEq) && (u1 > floorf(u1))) {
      real32_T q;
      q = fabsf(u0 / u1);
      yEq = !(fabsf(q - floorf(q + 0.5F)) > FLT_EPSILON * q);
    }

    if (yEq) {
      y = u1 * 0.0F;
    } else if ((u0 < 0.0F) != (u1 < 0.0F)) {
      y += u1;
    }
  }

  return y;
}

/* Output and update for atomic system: '<Root>/Current_Loop' */
void IF_Control_Current_Loop(void)
{
  real32_T rtb_Add2;
  real32_T rtb_Add3;
  real32_T rtb_IntegralGain;
  real32_T rtb_IntegralGain_i;
  real32_T rtb_Saturation;
  real32_T rtb_Sum4;
  real32_T rtb_Sum6;
  real32_T rtb_convert_pu;
  uint16_T rtb_Get_Integer;

  /* Outputs for Atomic SubSystem: '<S2>/Clarke Transform' */
  /* Gain: '<S5>/one_by_sqrt3' incorporates:
   *  Inport: '<Root>/Iabc'
   *  Sum: '<S5>/a_plus_2b'
   */
  rtb_Add3 = ((IF_Control_U.iabc[0] + IF_Control_U.iabc[1]) + IF_Control_U.iabc
              [1]) * 0.577350259F;

  /* End of Outputs for SubSystem: '<S2>/Clarke Transform' */

  /* Outputs for IfAction SubSystem: '<S3>/Switch Case Action Subsystem1' incorporates:
   *  ActionPort: '<S122>/Action Port'
   */
  /* SwitchCase: '<S3>/Switch Case' incorporates:
   *  Constant: '<S122>/Constant1'
   *  Constant: '<S122>/Constant2'
   *  Constant: '<S122>/Constant3'
   *  Constant: '<S122>/Constant4'
   *  DiscreteIntegrator: '<S122>/Discrete-Time Integrator'
   *  DiscreteIntegrator: '<S122>/Discrete-Time Integrator1'
   *  Gain: '<S122>/f_2_We'
   *  Gain: '<S122>/n_2_f'
   *  Inport: '<Root>/Iq_Set'
   *  Inport: '<Root>/Target_Speed'
   *  Math: '<S122>/Mod'
   *  Merge: '<S3>/Merge'
   *  Merge: '<S3>/Merge1'
   *  Merge: '<S3>/Merge2'
   *  Product: '<S122>/Divide'
   *  Saturate: '<S122>/Saturation'
   *  SignalConversion generated from: '<S122>/Iq_Set'
   *  SignalConversion generated from: '<S122>/Id'
   *  SignalConversion generated from: '<S122>/Theta'
   *  Sum: '<S122>/Add'
   * */
  IF_Control_B.Merge2 = 0.0F;
  IF_Control_B.Merge1 = rt_modf_snf(IF_Control_DW.DiscreteTimeIntegrator1_DSTATE
    - 1.57079637F, 6.28318548F);
  IF_Control_B.Merge = IF_Control_U.iq;
  if (IF_Control_DW.DiscreteTimeIntegrator_DSTATE > 1200.0F) {
    rtb_Add2 = 1200.0F;
  } else if (IF_Control_DW.DiscreteTimeIntegrator_DSTATE < 0.0F) {
    rtb_Add2 = 0.0F;
  } else {
    rtb_Add2 = IF_Control_DW.DiscreteTimeIntegrator_DSTATE;
  }

  IF_Control_DW.DiscreteTimeIntegrator1_DSTATE += 0.116666667F * rtb_Add2 *
    6.28318548F * 5.0E-5F;
  IF_Control_DW.DiscreteTimeIntegrator_DSTATE += IF_Control_U.speed / 3.0F *
    5.0E-5F;

  /* End of SwitchCase: '<S3>/Switch Case' */
  /* End of Outputs for SubSystem: '<S3>/Switch Case Action Subsystem1' */

  /* Gain: '<S117>/convert_pu' */
  rtb_convert_pu = 0.159154937F * IF_Control_B.Merge1;

  /* Outputs for IfAction SubSystem: '<S117>/If Action Subsystem1' incorporates:
   *  ActionPort: '<S120>/Action Port'
   */
  /* If: '<S117>/If' incorporates:
   *  DataTypeConversion: '<S120>/Convert_back'
   *  DataTypeConversion: '<S120>/Convert_uint16'
   *  Gain: '<S11>/indexing'
   *  Sum: '<S120>/Sum'
   */
  rtb_Add2 = truncf(rtb_convert_pu);
  if (rtIsNaNF(rtb_Add2)) {
    rtb_Add2 = 0.0F;
  } else {
    rtb_Add2 = fmodf(rtb_Add2, 65536.0F);
  }

  rtb_convert_pu = (rtb_convert_pu - (real32_T)(uint16_T)rtb_Add2) * 800.0F;

  /* End of If: '<S117>/If' */
  /* End of Outputs for SubSystem: '<S117>/If Action Subsystem1' */

  /* DataTypeConversion: '<S11>/Get_Integer' */
  rtb_Add2 = truncf(rtb_convert_pu);
  if (rtIsNaNF(rtb_Add2)) {
    rtb_Add2 = 0.0F;
  } else {
    rtb_Add2 = fmodf(rtb_Add2, 65536.0F);
  }

  rtb_Get_Integer = (uint16_T)(rtb_Add2 < 0.0F ? (int32_T)(uint16_T)-(int16_T)
    (uint16_T)-rtb_Add2 : (int32_T)(uint16_T)rtb_Add2);

  /* End of DataTypeConversion: '<S11>/Get_Integer' */

  /* Sum: '<S11>/Sum2' incorporates:
   *  DataTypeConversion: '<S11>/Data Type Conversion1'
   */
  rtb_Add2 = rtb_convert_pu - (real32_T)rtb_Get_Integer;

  /* Sum: '<S116>/Sum4' incorporates:
   *  Constant: '<S11>/offset'
   *  Constant: '<S11>/sine_table_values'
   *  Product: '<S116>/Product'
   *  Selector: '<S11>/Lookup'
   *  Sum: '<S116>/Sum3'
   *  Sum: '<S11>/Sum'
   */
  rtb_Sum4 = (IF_Control_ConstP.sine_table_values_Value[(int32_T)
              (rtb_Get_Integer + 1U)] -
              IF_Control_ConstP.sine_table_values_Value[rtb_Get_Integer]) *
    rtb_Add2 + IF_Control_ConstP.sine_table_values_Value[rtb_Get_Integer];

  /* Sum: '<S116>/Sum6' incorporates:
   *  Constant: '<S11>/offset'
   *  Constant: '<S11>/sine_table_values'
   *  Product: '<S116>/Product1'
   *  Selector: '<S11>/Lookup'
   *  Sum: '<S116>/Sum5'
   *  Sum: '<S11>/Sum'
   */
  rtb_Sum6 = (IF_Control_ConstP.sine_table_values_Value[(int32_T)
              (rtb_Get_Integer + 201U)] -
              IF_Control_ConstP.sine_table_values_Value[(int32_T)
              (rtb_Get_Integer + 200U)]) * rtb_Add2 +
    IF_Control_ConstP.sine_table_values_Value[(int32_T)(rtb_Get_Integer + 200U)];

  /* Outputs for Atomic SubSystem: '<S2>/Park Transform' */
  /* Outputs for Atomic SubSystem: '<S2>/Clarke Transform' */
  /* Switch: '<S115>/Switch' incorporates:
   *  AlgorithmDescriptorDelegate generated from: '<S5>/a16'
   *  Inport: '<Root>/Iabc'
   *  Product: '<S10>/acos'
   *  Product: '<S10>/bsin'
   *  Sum: '<S10>/sum_Ds'
   */
  rtb_convert_pu = IF_Control_U.iabc[0] * rtb_Sum6 + rtb_Add3 * rtb_Sum4;

  /* End of Outputs for SubSystem: '<S2>/Clarke Transform' */

  /* Gain: '<S40>/Integral Gain' incorporates:
   *  AlgorithmDescriptorDelegate generated from: '<S10>/a16'
   *  Sum: '<S2>/Sum'
   */
  rtb_IntegralGain = (0.0F - rtb_convert_pu) * 550.0F;

  /* Sum: '<S52>/Sum' incorporates:
   *  AlgorithmDescriptorDelegate generated from: '<S10>/a16'
   *  DiscreteIntegrator: '<S43>/Integrator'
   *  Gain: '<S48>/Proportional Gain'
   *  Sum: '<S2>/Sum'
   */
  rtb_Saturation = (0.0F - rtb_convert_pu) * 0.09F +
    IF_Control_DW.Integrator_DSTATE;

  /* End of Outputs for SubSystem: '<S2>/Park Transform' */

  /* Saturate: '<S50>/Saturation' */
  if (rtb_Saturation > 6.92820311F) {
    rtb_Saturation = 6.92820311F;
  } else if (rtb_Saturation < -6.92820311F) {
    rtb_Saturation = -6.92820311F;
  }

  /* End of Saturate: '<S50>/Saturation' */

  /* Outputs for Atomic SubSystem: '<S2>/Park Transform' */
  /* Outputs for Atomic SubSystem: '<S2>/Clarke Transform' */
  /* Sum: '<S2>/Sum1' incorporates:
   *  AlgorithmDescriptorDelegate generated from: '<S5>/a16'
   *  Inport: '<Root>/Iabc'
   *  Product: '<S10>/asin'
   *  Product: '<S10>/bcos'
   *  Sum: '<S10>/sum_Qs'
   */
  rtb_Add2 = IF_Control_B.Merge - (rtb_Add3 * rtb_Sum6 - IF_Control_U.iabc[0] *
    rtb_Sum4);

  /* End of Outputs for SubSystem: '<S2>/Clarke Transform' */
  /* End of Outputs for SubSystem: '<S2>/Park Transform' */

  /* Gain: '<S88>/Integral Gain' */
  rtb_IntegralGain_i = 550.0F * rtb_Add2;

  /* Sum: '<S100>/Sum' incorporates:
   *  DiscreteIntegrator: '<S91>/Integrator'
   *  Gain: '<S96>/Proportional Gain'
   */
  rtb_Add3 = 0.09F * rtb_Add2 + IF_Control_DW.Integrator_DSTATE_l;

  /* Saturate: '<S98>/Saturation' */
  if (rtb_Add3 > 6.92820311F) {
    rtb_Add3 = 6.92820311F;
  } else if (rtb_Add3 < -6.92820311F) {
    rtb_Add3 = -6.92820311F;
  }

  /* End of Saturate: '<S98>/Saturation' */

  /* Outputs for Atomic SubSystem: '<S2>/Inverse Park Transform' */
  /* Switch: '<S108>/Switch' incorporates:
   *  Product: '<S8>/dcos'
   *  Product: '<S8>/qsin'
   *  Sum: '<S8>/sum_alpha'
   */
  rtb_convert_pu = rtb_Saturation * rtb_Sum6 - rtb_Add3 * rtb_Sum4;

  /* Gain: '<S114>/one_by_two' incorporates:
   *  AlgorithmDescriptorDelegate generated from: '<S8>/a16'
   */
  rtb_Add2 = 0.5F * rtb_convert_pu;

  /* Gain: '<S114>/sqrt3_by_two' incorporates:
   *  Product: '<S8>/dsin'
   *  Product: '<S8>/qcos'
   *  Sum: '<S8>/sum_beta'
   */
  rtb_Add3 = (rtb_Add3 * rtb_Sum6 + rtb_Saturation * rtb_Sum4) * 0.866025388F;

  /* End of Outputs for SubSystem: '<S2>/Inverse Park Transform' */

  /* Sum: '<S114>/add_b' */
  rtb_Sum4 = rtb_Add3 - rtb_Add2;

  /* Sum: '<S114>/add_c' */
  rtb_Add2 = (0.0F - rtb_Add2) - rtb_Add3;

  /* Outputs for Atomic SubSystem: '<S2>/Inverse Park Transform' */
  /* Gain: '<S112>/one_by_two' incorporates:
   *  AlgorithmDescriptorDelegate generated from: '<S8>/a16'
   *  MinMax: '<S112>/Max'
   *  MinMax: '<S112>/Min'
   *  Sum: '<S112>/Add'
   */
  rtb_Add3 = (fmaxf(fmaxf(rtb_convert_pu, rtb_Sum4), rtb_Add2) + fminf(fminf
    (rtb_convert_pu, rtb_Sum4), rtb_Add2)) * -0.5F;

  /* Sum: '<S4>/Add' incorporates:
   *  AlgorithmDescriptorDelegate generated from: '<S8>/a16'
   *  Constant: '<S4>/Constant'
   *  Gain: '<S111>/Gain'
   *  Gain: '<S4>/Gain'
   *  Sum: '<S111>/Add1'
   *  Sum: '<S111>/Add2'
   *  Sum: '<S111>/Add3'
   */
  IF_Control_B.Add[0] = (rtb_convert_pu + rtb_Add3) * 1.15470052F *
    0.0721687824F + 0.5F;

  /* End of Outputs for SubSystem: '<S2>/Inverse Park Transform' */
  IF_Control_B.Add[1] = (rtb_Sum4 + rtb_Add3) * 1.15470052F * 0.0721687824F +
    0.5F;
  IF_Control_B.Add[2] = (rtb_Add3 + rtb_Add2) * 1.15470052F * 0.0721687824F +
    0.5F;

  /* Update for DiscreteIntegrator: '<S43>/Integrator' */
  IF_Control_DW.Integrator_DSTATE += 5.0E-5F * rtb_IntegralGain;

  /* Update for DiscreteIntegrator: '<S91>/Integrator' */
  IF_Control_DW.Integrator_DSTATE_l += 5.0E-5F * rtb_IntegralGain_i;
}

/*
 * File trailer for generated code.
 *
 * [EOF]
 */
